#pragma GCC optimize ("Og")

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include <jtag/adapter.h>
#include <jtag/interface.h>
#include <transport/transport.h>
#include "bitbang.h"

#include <libusb.h>
//#include <libusb-1.0/libusb.h>

//定义USB的处理
#define dev_vid 0x33aa
#define dev_pid 0x0120
#define SEND_EP 0x02
#define RECV_EP 0x81
#define USB_TIMEOUT 1000

#define claim       0
#define usb_config  1


#define CLOCK_IDLE() 0

//定义缓存处理
#define write_buf_max 64
static uint8_t write_buf[write_buf_max];

static int last_tdi = 0;
static int write_buf_pos = 0;

//命令处理
static uint8_t cmd_map[0x100];
#define cmd_map_set(cmd,len,tdo_len) cmd_map[cmd] = ((len)|((tdo_len)<<4))
#define cmd_map_get_bit_len(cmd)    (cmd_map[cmd] & 0xf)
#define cmd_map_tdo_read(cmd)       ((cmd_map[cmd]>>4)&0xf)


static int gwu2x_init(void);
static int gwu2x_quit(void);
static int gwu2x_reset(int trst, int srst);
#define blink(on)

#define INIT_USB_CTX if(ctx.usb_ctx==0){ \
        libusb_init(&ctx.usb_ctx); \
    }

typedef struct gwu2x_ctx_t{
    libusb_context*usb_ctx;
    libusb_device_handle *dev;
}gwu2x_ctx_t;

static gwu2x_ctx_t ctx;
static int write_usb_byte(uint8_t d);
static int usb_read_bytes(void*buf,int sz);
//static int j_write(int tck,int tms,int tdi);
static int j_write_tms(int tms,int tdi,int is_end);
static int j_write_tdi(uint8_t*buffer,int size,int tdo_read,int last_tms);
static int j_read_tdo(uint8_t*buffer,int size);
static int usb_flush(void);

static libusb_device_handle* usb_list_proc(int vid,int pid,int *error){
    INIT_USB_CTX;
    struct libusb_device **list;
    struct libusb_device_descriptor desc;
    int cnt = libusb_get_device_list(ctx.usb_ctx,&list);
    int i;
    int err;
    if(cnt<0){
        printf("%s","error!\n");
    }
    for(i=0;i<cnt;i++){
        struct libusb_device *device = list[i];
        err = libusb_get_device_descriptor(device, &desc);
        if (err != LIBUSB_SUCCESS) {
			printf("libusb_get_device_descriptor() failed with %s", libusb_error_name(err));
			continue;
		}

        //printf("vid:%04X,pio:%04X\n",desc.idVendor,desc.idProduct);
        if(desc.idVendor==vid&&desc.idProduct==pid){
            libusb_device_handle *h = 0;//libusb_open_device_with_vid_pid(ctx,vid,pid);
            err = libusb_open(device,&h);
            *error = err;
            libusb_free_device_list(list,1);
            return h;
        }
    }
    libusb_free_device_list(list,1);
    return 0;
}

static int gwu2x_init(void){

    //初始化命令的信息
    cmd_map_set(0x5b,1,0);
    cmd_map_set(0x6b,1,0);
    cmd_map_set(0x6d,1,0);
    cmd_map_set(0x7b,8,0);
    cmd_map_set(0x7d,8,0);

    //tdo
    cmd_map_set(0x5b+1,1,1);
    cmd_map_set(0x6b+1,1,1);
    cmd_map_set(0x6d+1,1,1);
    cmd_map_set(0x7b+1,8,8);
    cmd_map_set(0x7d+1,8,8);

    //tck

    //初始化usb
    INIT_USB_CTX;
    ctx.dev = libusb_open_device_with_vid_pid(ctx.usb_ctx,dev_vid,dev_pid);
    if(!ctx.dev){
        int err = 0;
        ctx.dev = usb_list_proc(dev_vid,dev_pid,&err);
    }
    if(!ctx.dev){
        LOG_ERROR("open gwu2x error!");
        return ERROR_FAIL;
    }

    int config = 0;
    int err = 0;


    if((err=libusb_get_configuration(ctx.dev,&config))){
        LOG_ERROR("get config %s",libusb_error_name(err));
        return ERROR_FAIL;
    }

    if((err=libusb_set_configuration(ctx.dev,config))){
        LOG_ERROR("set config %s",libusb_error_name(err));
        return ERROR_FAIL;
    }

    if((err=libusb_set_configuration(ctx.dev,usb_config))){
        LOG_ERROR("set config %d %s",usb_config,libusb_error_name(err));
        return ERROR_FAIL;
    }

    if((err=libusb_claim_interface(ctx.dev,claim))){
        LOG_ERROR("claim %d %s",claim,libusb_error_name(err));
        return ERROR_FAIL;
    }


    //初始化IO状态
    write_usb_byte(0x20);
    write_usb_byte(0x07);       //tck,tms,tdi
    write_usb_byte(0x01);       //默认IDLE为低电平
    usb_flush();

    return ERROR_OK;
}

static int gwu2x_quit(void){
    libusb_close(ctx.dev);
    ctx.dev = 0;
    return ERROR_OK;
}

static int gwu2x_reset(int trst, int srst){
    return ERROR_OK;
}

static int tms_buf = 0,tms_buf_pos = 0;

static int usb_flush(void){
    // if(tms_buf_pos>0){
    //     //tms缓存还有数据
    //     write_usb_byte(0x5b);
    //     write_usb_byte(tms_buf_pos-1);
    //     write_usb_byte(tms_buf);
    //     tms_buf = 0;
    //     tms_buf_pos = 0;
    // }

    if(write_buf_pos>0){
        libusb_bulk_transfer(ctx.dev,SEND_EP,write_buf,write_buf_pos,0,USB_TIMEOUT);
        write_buf_pos = 0;
    }
    return ERROR_OK;
}

static int usb_recv_buf_pos = 0;
static uint8_t usb_recv_buf[1024];

static int usb_read_bytes(void*buf,int sz){
    if(usb_recv_buf_pos>0){
        int copy_len = sz;
        if(copy_len>usb_recv_buf_pos) copy_len = usb_recv_buf_pos;
        memcpy(buf,usb_recv_buf,copy_len);
        if(usb_recv_buf_pos-copy_len!=0){
            memmove(usb_recv_buf,usb_recv_buf+copy_len,usb_recv_buf_pos-copy_len);
        }
        usb_recv_buf_pos -= copy_len;
        sz -= copy_len;
        buf = (uint8_t*)buf + copy_len;
    }

    if(sz>0){
        int alen = 0;
        libusb_bulk_transfer(ctx.dev,RECV_EP,buf,sz,&alen,USB_TIMEOUT);
    }
    return ERROR_OK;
}

static int usb_read_bytes_totmp(int sz){
    if(sz>0){
        int alen = 0;
        libusb_bulk_transfer(ctx.dev,RECV_EP,usb_recv_buf+usb_recv_buf_pos,sz,&alen,USB_TIMEOUT);
        usb_recv_buf_pos += alen;
    }
    return ERROR_OK;
}

static int write_usb_byte(uint8_t d){
    write_buf[write_buf_pos++] = d;
    if(write_buf_pos==write_buf_max){
        usb_flush();
    }
    return ERROR_OK;
}

static int write_usb_remaining(void){
    return write_buf_max - write_buf_pos;
}

static int write_usb_cmd_and_len(uint8_t cmd,uint8_t len){
    int bit_len = cmd_map_get_bit_len(cmd) * (len+1);
    int byte_len = bit_len / 8;
    if(bit_len&7) byte_len += 1;        //需要填充

    if(write_buf_pos + byte_len + 2 >= write_buf_max){
        //溢出,刷新usb
        LOG_INFO("usb buffer overflow flush:%d",write_buf_pos);
        usb_flush();
    }

    write_usb_byte(cmd);
    write_usb_byte(len);
    return ERROR_OK;
}

static int j_write_tms(int tms,int tdi,int is_end){
    //缓存溢出,写到usb缓存上
    tms_buf |= tms<<tms_buf_pos++;
    if(tms_buf_pos==7 || is_end){
        write_usb_cmd_and_len(0x5b,tms_buf_pos-1);
        write_usb_byte(tms_buf | (tdi<<tms_buf_pos));
        tms_buf_pos = 0;
        tms_buf = 0;
    }

    return ERROR_OK;
}

static int j_buf32_is(uint32_t*buffer32,uint32_t value,uint32_t*bufferEnd){
    int len = 0;
    while((buffer32 < bufferEnd) && *buffer32==value){
        len += 1;
        buffer32 += 1;
    }
    return len;
}

static int j_write_tdi(uint8_t*buffer,int size,int tdo_read,int last_tms){
    int pos = 0;
    /*
    最后一位需要把TMS与TDI并行处理
    */

    LOG_INFO("scan:%d,%d,%p",size,tdo_read,buffer);
    if(buffer && size>32){
        //对于超长的数据处理
        uint32_t *buffer32 = (uint32_t*)buffer;

        //需要32位对齐比较
        uint32_t *bufferEnd = (uint32_t*)(buffer + (size&~0x1f) /8);
        int len = 0;
        int d_data = 0;
        int bit_offset = 0;

        while(size>8 && buffer32<bufferEnd){
            if((len=j_buf32_is(buffer32,0,bufferEnd))){
                //写入8位0
                d_data = 0;
            }
            else if((len=j_buf32_is(buffer32,~0,bufferEnd))){
                //写入8位1
                d_data = ~0;
            }
            else{
                break;
            }

            int bit_len = len*4*8;

            if(bit_len>0){
                if(tdo_read && bit_len>0x40*8){
                    //可能会读取数据
                    bit_len = 0x40*8;
                }

                write_usb_cmd_and_len(0x7b+tdo_read,0);
                write_usb_byte(d_data);
                last_tdi = d_data & 1;
                //写入剩余的数据
                //j_write_tdi(0,bit_len-8,tdo_read,last_tms);
                if(write_usb_remaining()<3)
                    usb_flush();

                if(size==bit_len){
                    //最后一个数据时
                    bit_len -= 8;       //留下位对齐的1字节数据
                }

                write_usb_cmd_and_len(0x9b+tdo_read,bit_len-8-1);
                write_usb_byte((bit_len-8-1)>>8);

                if(tdo_read && bit_len>=0x40*8){
                    usb_flush();
                    usb_read_bytes_totmp(bit_len/8);
                }

                buffer32    += bit_len/32;
                bit_offset  += bit_len;
            }

            LOG_INFO("write duplicate data:%d,%d",d_data,len*4*8);

            //数据处理
            size -= bit_len;
        }
        buffer += bit_offset / 8;
    }

    LOG_INFO("tdi write:%d",size);

    if(buffer && size>0){
        //传输tdi数据

        //最后一位tdi
        int end_tdi = (buffer[size/8] >> ((size - 1) %8)) & 1;

        //size -= 1;
        int align_8 = ((size-1) & ~7);
        int bit_len =  (size-1) & 7;

        if(align_8>0){
            //8位对齐

            //需要pos从0 到 remaining_write 的位置
            int remaining_write = align_8/8;

            //循环写入
            while(pos<remaining_write){
                //查看usb缓存还能写入多少数据
                int remaining = write_usb_remaining();
                int byte_len = remaining_write - pos;

                if(byte_len+2>remaining){
                    byte_len = remaining - 2;
                }
                if(byte_len<=0){
                    //需要刷新usb缓存
                    usb_flush();
                    continue;
                }

                LOG_INFO("usb block write:%d/%d(%d)(usb:%d)",pos,remaining_write,byte_len,remaining);

                write_usb_cmd_and_len(0x7b+tdo_read,byte_len-1);

                while(byte_len>0){
                    write_usb_byte(buffer[pos++]);
                    byte_len -= 1;
                }
            }
        }

        if(bit_len>0){
            //tdi位传输
            write_usb_cmd_and_len(0x6b+tdo_read,bit_len-1);
            write_usb_byte(buffer[pos++]);
        }

        //处理最后一位tdi数据时,tms需要拉高
        write_usb_cmd_and_len(0x5b+tdo_read,0);
        write_usb_byte(last_tms | (end_tdi<<7));
        last_tdi = end_tdi;
    }
    else if(tdo_read && size>0){
        //tdi需要设置为0
        //不传输tdi,但需要读取tdo

        //不足3字节,需要刷新
        if(write_usb_remaining()<3)
            usb_flush();


        //最后一个数据需要拉高tms
        write_usb_cmd_and_len(0x9c,size-2);
        write_usb_byte((size-2)>>8);

        write_usb_cmd_and_len(0x5b+tdo_read,0);
        write_usb_byte(last_tms | (last_tdi<<7));
    }
    else{
        //不做任何事
        //重复数据.但
    }

    //强制对齐读取tdo
    if(((size&7)!=0) && tdo_read){
        write_usb_byte(0x8b);
    }

    usb_flush();

    return ERROR_OK;
}

static int j_read_tdo(uint8_t*buffer,int size){
    usb_read_bytes(buffer,size);
    return ERROR_OK;
}

static int state_move(int skip)
{
	uint8_t tms_scan = tap_get_tms_path(tap_get_state(), tap_get_end_state());
	int tms_count = tap_get_tms_path_len(tap_get_state(), tap_get_end_state());

    //最多8位
    // tdi = 0
    if(tms_count>7){
        write_usb_cmd_and_len(0x5b,0);
        write_usb_byte(tms_scan&1);
        tms_scan>>=1;
    }
    write_usb_cmd_and_len(0x5b,tms_count-1);
    write_usb_byte(tms_scan);

	tap_set_state(tap_get_end_state());
	return ERROR_OK;
}

static int path_move(struct pathmove_command *cmd)
{
	int num_states = cmd->num_states;
	int state_count;
	int tms = 0;

	state_count = 0;
	while (num_states) {
		if (tap_state_transition(tap_get_state(), false) == cmd->path[state_count])
			tms = 0;
		else if (tap_state_transition(tap_get_state(), true) == cmd->path[state_count])
			tms = 1;
		else {
			LOG_ERROR("BUG: %s -> %s isn't a valid TAP transition",
				tap_state_name(tap_get_state()),
				tap_state_name(cmd->path[state_count]));
			exit(-1);
		}

        //tdi = 0
        j_write_tms(tms,0,num_states==1);

		tap_set_state(cmd->path[state_count]);
		state_count++;
		num_states--;
	}

    //j_write(CLOCK_IDLE(),tms,0);

	tap_set_end_state(tap_get_state());
	return ERROR_OK;
}

static void end_state(tap_state_t state)
{
	assert(tap_is_state_stable(state));
	tap_set_end_state(state);
}

static int runtest(int num_cycles)
{
	int i;

	tap_state_t saved_end_state = tap_get_end_state();

	/* only do a state_move when we're not already in IDLE */
	if (tap_get_state() != TAP_IDLE) {
		end_state(TAP_IDLE);
		if (state_move(0) != ERROR_OK)
			return ERROR_FAIL;
	}

	/* execute num_cycles */
	for (i = 0; i < num_cycles; i++) {
		j_write_tms(0,0,i==num_cycles-1);
	}

    //j_write(CLOCK_IDLE(),0,0);

	/* finish in end_state */
	end_state(saved_end_state);
	if (tap_get_state() != tap_get_end_state())
		if (state_move(0) != ERROR_OK)
			return ERROR_FAIL;

	return ERROR_OK;
}

static int stableclocks(int num_cycles)
{
	int tms = (tap_get_state() == TAP_RESET ? 1 : 0);
	int i;

	/* send num_cycles clocks onto the cable */
	for (i = 0; i < num_cycles; i++) {
        j_write_tms(tms,0,i==num_cycles-1);
	}

	return ERROR_OK;
}

static int scan(bool ir_scan, enum scan_type type, uint8_t *buffer,
		unsigned scan_size)
{
	tap_state_t saved_end_state = tap_get_end_state();

	if (!((!ir_scan &&
			(tap_get_state() == TAP_DRSHIFT)) ||
			(ir_scan && (tap_get_state() == TAP_IRSHIFT)))) {
		if (ir_scan)
			end_state(TAP_IRSHIFT);
		else
			end_state(TAP_DRSHIFT);

		if (state_move(0) != ERROR_OK)
			return ERROR_FAIL;
		end_state(saved_end_state);
	}


    //last_tms为最后一个状态,如果状态没有切换.则继续DR-SHIFT
    j_write_tdi((type!=SCAN_IN)?buffer:0,scan_size,type!=SCAN_OUT,tap_get_state()!=tap_get_end_state() );

    if(type!=SCAN_OUT){
        //获取数据
        j_read_tdo(buffer,(scan_size+7)/8);
    }

	if (tap_get_state() != tap_get_end_state()) {


        //此时的状态应该是DR_EXIT1, 数据传送的最后一位时.tms被拉高
        //last_tms 被设置为1
        tap_set_state(tap_state_transition(tap_get_state(), 1));

        //需要额外发送一个tms来进入DR_PAUSE
        j_write_tms(0,0,1);

        //切换状态
        tap_set_state(tap_state_transition(tap_get_state(), 0));
	}

    //进行tms最后操作时,可能会进入相同的状态
    if (tap_get_state() != tap_get_end_state() ){
        if (state_move(1) != ERROR_OK)
            return ERROR_FAIL;
    }

	return ERROR_OK;
}

static int execute_tms(struct jtag_command *cmd)
{
	unsigned num_bits = cmd->cmd.tms->num_bits;
	const uint8_t *bits = cmd->cmd.tms->bits;

	LOG_DEBUG_IO("TMS: %d bits", num_bits);

	int tms = 0;
	for (unsigned i = 0; i < num_bits; i++) {
		tms = ((bits[i/8] >> (i % 8)) & 1);
        //tms=0
        j_write_tms(tms,0,i==num_bits-1);
	}

    //j_write(CLOCK_IDLE(),tms,0);

	return ERROR_OK;
}

int gwu2x_execute_queue(void){
    struct jtag_command *cmd = jtag_command_queue;	/* currently processed command */
    int scan_size;
	enum scan_type type;
	uint8_t *buffer;
	int retval = ERROR_OK;

    blink(1);

    //处理代码
    while(cmd){
        switch(cmd->type){
            case JTAG_RUNTEST:{
                runtest(cmd->cmd.runtest->num_cycles);                      //o
            }
            break;

            case JTAG_STABLECLOCKS:{
                stableclocks(cmd->cmd.stableclocks->num_cycles);            //o
            }
            break;

            case JTAG_TLR_RESET: { state_move(0); }break;                    //o
            case JTAG_PATHMOVE: { path_move(cmd->cmd.pathmove); } break;     //o
            case JTAG_SCAN:{
                end_state(cmd->cmd.scan->end_state);
                scan_size = jtag_build_buffer(cmd->cmd.scan, &buffer);
                type = jtag_scan_type(cmd->cmd.scan);
                if (scan(cmd->cmd.scan->ir_scan, type, buffer,
							scan_size) != ERROR_OK)
					return ERROR_FAIL;
                if (jtag_read_buffer(buffer, cmd->cmd.scan) != ERROR_OK)
					retval = ERROR_JTAG_QUEUE_FAILED;
                free(buffer);
                break;
            }

            case JTAG_SLEEP:{
                jtag_sleep(cmd->cmd.sleep->us);
                break;
            }

            case JTAG_TMS:{
                retval = execute_tms(cmd);
                break;
            }
            default:
				LOG_ERROR("BUG: unknown JTAG command type encountered");
				exit(-1);
        }
        cmd = cmd->next;
    }

    blink(0);

    return retval;
}


static struct jtag_interface gwu2x_interface = {
	.execute_queue = gwu2x_execute_queue,
};

static int gwu2x_speed(int speed){
    if (speed == 0) {
		LOG_ERROR("RTCK not supported. Set nonzero \"adapter speed\".");
		return ERROR_JTAG_NOT_IMPLEMENTED;
	}
    //未找到指令
    return ERROR_OK;
}

static const char *const gwu2x_transport[] = { "jtag", NULL };
struct adapter_driver gwu2x_adapter_driver = {
	.name = "gwu2x",
	.transports = gwu2x_transport,

	.init = gwu2x_init,
	.quit = gwu2x_quit,
	.reset = gwu2x_reset,
    .speed = gwu2x_speed,

	.jtag_ops = &gwu2x_interface,
	//.swd_ops = &bitbang_swd,
};
